#ifndef pub_tf_h
#define pub_tf_h

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/Imu.h>
#include <Eigen/Dense>

// #include <queue>
#include <deque>

#include <memory>   // 智能指针
#include <thread>   // 多线程
#include <mutex>    // 加锁

#include <opencv2/opencv.hpp>
#include <cv_bridge/cv_bridge.h>

// std::queue<std::pair<double, Eigen::Vector3d>> AccBf;
// std::queue<std::pair<double, Eigen::Vector3d>> GyrBf;
// std::queue<std::pair<double, cv::Mat>> ImgBf;

std::mutex imu_lock;
std::mutex image_lock;

std::deque<sensor_msgs::Imu> imuQueue;
std::deque<std::pair<double, cv::Mat>> imageQueue;

std::queue<sensor_msgs::ImageConstPtr> image_buf;
std::queue<sensor_msgs::ImuConstPtr> imu_buf;

ros::Subscriber sub_imu;
ros::Subscriber sub_image;


cv::Mat imageLast;
sensor_msgs::Imu imuMsgLast;
double timeImageLast, timeImageCur;




void callbackImu(const sensor_msgs::ImuConstPtr &msg_imu);
void callbackImage(const sensor_msgs::ImageConstPtr &msg_image);
void imuProcess();
void imageProcess();



#endif